#include<global_planner/AnyAngleInterface.h>
#include<costmap_2d/cost_values.h>
#include<algorithm>
namespace global_planner {
    AnyAngleExpansion::AnyAngleExpansion(PotentialCalculator* p_calc, int xs, int ys) :
        Expander(p_calc, xs, ys) {
    }
    bool AnyAngleExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,
                                        int cycles, float* potential) {}
    bool AnyAngleExpansion::path_find(unsigned char* costs,double resolution, unsigned int start_x, unsigned int start_y, 
                            unsigned int end_x, unsigned int end_y, std::vector<std::vector<unsigned int> > *path){

        //ROS_INFO("map_size:%f",WIDTH*0.05);
        int Smap_start[2] = {0,0};
		int widthNow = 0;
		int heightNow = 0;
        int start_grid[2] = {int(start_x),int(start_y)};
        int goal_grid[2] = {int(end_x),int(end_y)};
        const_shift_s[0] = 2.0;
        const_shift_s[1] = 2.0;
        const_shift_e[0] = 2.0;
        const_shift_e[1] = 2.0;
        std::vector<std::vector<unsigned int> >().swap(*path);
        while(0 == path->size() && (heightNow <= ny_ || widthNow <= nx_))
        {
            const_shift_s[0] *= 2;
			const_shift_s[1] *= 2;
			const_shift_e[0] *= 2;
			const_shift_e[1] *= 2;
			heightNow = ((const_shift_s[1]+const_shift_e[1])/2)/resolution + abs(start_grid[1]-goal_grid[1]);
			widthNow = ((const_shift_s[0]+const_shift_e[0])/2)/resolution + abs(start_grid[0]-goal_grid[0]);
            std::vector<std::vector<unsigned int> >().swap(*path);
            get_path(costs,resolution,start_grid,goal_grid,Smap_start,path);
        }
/*        do{
            std::vector<std::vector<unsigned int> >().swap(*path);
            get_path(costs,resolution,start_grid,goal_grid,Smap_start,path);

            const_shift_s[0] *= 2;
			const_shift_s[1] *= 2;
			const_shift_e[0] *= 2;
			const_shift_e[1] *= 2;
			heightNow = ((const_shift_s[1]+const_shift_e[1])/2)/resolution + abs(start_grid[1]-goal_grid[1]);
			widthNow = ((const_shift_s[0]+const_shift_e[0])/2)/resolution + abs(start_grid[0]-goal_grid[0]);

            if(path->size() == 0)   
            {
                get_path(costs,resolution,start_grid,goal_grid,Smap_start,path);
            }

        }while(0 == path->size() && (heightNow <= ny_ || widthNow <= nx_));
*/        if(path->size())
            return true;
        else
            return false;
	}
    bool AnyAngleExpansion::get_path(unsigned char* costs, double resolution, int startPoint[2],
        int stopPoint[2],int Smap_start[2],std::vector<std::vector<unsigned int> > *path)
    {
        Smap_start[0] = std::min(startPoint[0],stopPoint[0]) - int(const_shift_s[0]/resolution);
        Smap_start[1] = std::min(startPoint[1],stopPoint[1]) - int(const_shift_s[1]/resolution);
        int Smap_end[2]  = {std::max(startPoint[0],stopPoint[0]) + int(const_shift_e[0]/resolution),
            std::max(startPoint[1],stopPoint[1]) + int(const_shift_e[1]/resolution)};

        Smap_start[0] = Smap_start[0]>0?Smap_start[0]:0;
        Smap_start[1] = Smap_start[1]>0?Smap_start[1]:0;
        Smap_end[0] = Smap_end[0]<nx_?Smap_end[0]:nx_;
        Smap_end[1] = Smap_end[1]<ny_?Smap_end[1]:ny_;

        N_width = Smap_end[0] - Smap_start[0];
        N_height = Smap_end[1] - Smap_start[1];        

        std::vector<bool>().swap(mapData);
        unsigned long size = N_width*N_height;
	    mapData.resize(size,true);					//all free
        for(int j=0;j<N_height;j++){
            for(int i=0;i<N_width;i++){
                if(costs[(j+Smap_start[1])*nx_ + Smap_start[0]+i]>costmap_2d::FREE_SPACE){
                    mapData[j*N_width+i] = false;
                }
            }
        }
        int searchStart[2] = {startPoint[0]-Smap_start[0],startPoint[1]-Smap_start[1]};
	    int searchStop[2] = {stopPoint[0]-Smap_start[0],stopPoint[1]-Smap_start[1]};

        mapData[searchStart[1]*N_width+searchStart[0]] = true;
        mapData[searchStop[1]*N_width+searchStop[0]] = true;
        bool is_start_free = mapData[searchStart[1]*N_width+searchStart[0]];
        bool is_stop_free = mapData[searchStop[1]*N_width+searchStop[0]];
        if(!is_start_free )
        {
            ROS_WARN("start point is blocked");
            return false;
        }
        if(!is_stop_free )
        {
            ROS_WARN("stop point is blocked");
            return false;
        }

        AnyAngleAlgorithm* algorithms = new ThetaStar(mapData, N_width, N_height, LAZY_THETA_STAR);
        xyLoc s, g;
        s.x = searchStart[0];
        s.y = searchStart[1];
        g.x = searchStop[0];
        g.y = searchStop[1];        
        try{
            algorithms-> FindPath(s,g);
        }
        catch(...){
            std::cout<<"Find global Path Error ..."<<std::endl;
            return false;
        }
        //load path upturn
        int path_size = algorithms->smoothed_xyloc_path_.size();
        std::vector<std::vector<unsigned int> > path_;
        for(int i=0;i<path_size;i++)            //in order, goal is the last point
        {
            std::vector<unsigned int> tempPath;
            tempPath.emplace_back(algorithms->smoothed_xyloc_path_[i].x+Smap_start[0]);
            tempPath.emplace_back(algorithms->smoothed_xyloc_path_[i].y+Smap_start[1]);
            path_.emplace_back(tempPath);
        }
        //ROS_INFO("AngAngle_size:%d",path_size);
        if(path_size<1)
        {
            delete algorithms;
            return false;
        }
        else if(1 == path_size)
        {
            path->emplace_back(path_[0]);
            delete algorithms;
            return true;
        }
        else{}

        if(!isFilledPath())
        {
            *path = path_;
            return true;
        }
        
        for(int i=0;i<path_size-1;i++)
        {
            std::vector<int> judgeRouteX,judgeRouteY;
            bresenham(path_[i][0],path_[i][1],path_[i+1][0],path_[i+1][1],judgeRouteX,judgeRouteY);

            int judgeSize = judgeRouteX.size();
            int error_start = (path_[i][0]-judgeRouteX[0])*(path_[i][0]-judgeRouteX[0]) + 
                                (path_[i][1]-judgeRouteY[0])*(path_[i][1]-judgeRouteY[0]);
            int error_stop = (path_[i][0]-judgeRouteX[judgeSize-1])*(path_[i][0]-judgeRouteX[judgeSize-1]) + 
                                (path_[i][1]-judgeRouteY[judgeSize-1])*(path_[i][1]-judgeRouteY[judgeSize-1]);
            std::vector<int> selectRouteX;
            std::vector<int> selectRouteY;
            if(error_start < error_stop)
            {
                for(int m = 0;m<judgeSize;m++)
                {
                    selectRouteX.emplace_back(judgeRouteX[m]);
                    selectRouteY.emplace_back(judgeRouteY[m]);
                }
            }
            else
            {
                for(int m = 0;m<judgeSize;m++)
                {			
                    selectRouteX.emplace_back(judgeRouteX[judgeSize-1-m]);
                    selectRouteY.emplace_back(judgeRouteY[judgeSize-1-m]);
                }
            }
            int size = selectRouteX.size();
            for(int k = 1;k<size;k++)
            {
                std::vector<unsigned int> tempPath;
                tempPath.emplace_back(selectRouteX[k]);
                tempPath.emplace_back(selectRouteY[k]);
                path->emplace_back(tempPath);
            }
        }

        delete algorithms;
        return true;
    }

    void AnyAngleExpansion::bresenham(int x1,int y1,int x2,int y2,std::vector<int> &grid_free_x,std::vector<int> &grid_free_y)
    {
        if (x1 != x2)
        {
            float k = float(y2-y1)/float(x2-x1);
            int flag = 0;
            if (abs(k)>1.)
            {
                flag=1;
                k=1./k;
                int temp;
                temp = x1;	x1=y1;	y1=temp;// 	[y1 x1]=Swap(x1,y1);           
                temp = x2;	x2=y2;	y2=temp;//	[y2 x2]=Swap(x2,y2);
            }
            int mi = (x1<x2)?x1:x2;
            int ma = (x1>x2)?x1:x2;
            int s;
            if (mi == x1)
                s = y1;
            else
                s = y2;
            float d = 0;
            for(int i=mi;i<=ma;i++)
            {
                if (0 == flag)
                {
                    grid_free_x.emplace_back(i);
                    grid_free_y.emplace_back(s);
                }
                else
                {
                    grid_free_x.emplace_back(s);
                    grid_free_y.emplace_back(i);
                }
                d = d+k;
                if(d>=1.)
                {
                    d=d-1.;
                    s=s+1.;
        
                }
                else if(d<=-1.)
                {
                    d=d+1.;
                    s=s-1.;
                }
            }
        }
        else
        {
            int sign = (y1<y2)?1:-1;
            for(int i=0;i<=abs(y2-y1);i++)
            {
                grid_free_x.emplace_back(x1);
                grid_free_y.emplace_back(y1+sign*i);
            }
        }
    }
}
